#include "HitBox.hpp"


glm::vec3 get_vec3(glm::vec4 v){
	return vec3(v.x, v.y, v.z);
}

HitBox HitBox::getMovedHitBox(glm::mat4 positionMatrix) {
	glm::vec4 staticCorner(m_corner.x,m_corner.y,m_corner.z,1.0);
	glm::vec4 nc = positionMatrix * staticCorner;
	nc /= nc.w;
	// new dimension vector
	glm::vec4 p1 = glm::vec4(m_corner + m_dim1, 1.0);
	glm::vec4 p2 = glm::vec4(m_corner + m_dim2, 1.0);
	glm::vec4 p3 = glm::vec4(m_corner + m_dim3,1.0);
	glm::vec4 ndv1 = (positionMatrix * p1);// - nc;
	glm::vec4 ndv2 = (positionMatrix * p2);// - nc;
	glm::vec4 ndv3 = (positionMatrix * p3);// - nc;
	ndv1 /= ndv1.w; ndv2 /= ndv2.w; ndv3 /= ndv3.w;
	return HitBox(get_vec3(nc), get_vec3(ndv1 - nc), get_vec3(ndv2 - nc), get_vec3(ndv3 - nc));
};

void HitBox::render() {
    glColor3f(0.0f,0.0f,1.0f);
	vec3 cor = m_corner;
	vec3 dim1 = m_dim1;
	vec3 dim2 = m_dim2;
	vec3 dim3 = m_dim3;

	glBegin(GL_LINES);
	vec3 point1,point2;
	point1 = cor;
	point2 = cor + dim1;
	glVertex3f(point1.x, point1.y, point1.z);
	glVertex3f(point2.x, point2.y, point2.z);
	point1 = cor;
	point2 = cor + dim2;
	glVertex3f(point1.x, point1.y, point1.z);
	glVertex3f(point2.x, point2.y, point2.z);
	point1 = cor;
	point2 = cor + dim3;
	glVertex3f(point1.x, point1.y, point1.z);
	glVertex3f(point2.x, point2.y, point2.z);

	point1 = cor + dim2 + dim3;
  point2 = cor + dim1 + dim2 + dim3;
  glVertex3f(point1.x, point1.y, point1.z);
  glVertex3f(point2.x, point2.y, point2.z);
  point1 = cor + dim2 ;
  point2 = cor + dim3 + dim2;
  glVertex3f(point1.x, point1.y, point1.z);
  glVertex3f(point2.x, point2.y, point2.z);
  point1 = cor + dim2 + dim3;
  point2 = cor + dim3;
  glVertex3f(point1.x, point1.y, point1.z);
  glVertex3f(point2.x, point2.y, point2.z);

  point1 = cor + dim1 + dim3;
  point2 = cor + dim1 + dim2 + dim3;
  glVertex3f(point1.x, point1.y, point1.z);
  glVertex3f(point2.x, point2.y, point2.z);
  point1 = cor + dim1 ;
  point2 = cor + dim3 + dim1;
  glVertex3f(point1.x, point1.y, point1.z);
  glVertex3f(point2.x, point2.y, point2.z);
  point1 = cor + dim1 + dim3;
  point2 = cor + dim3;
  glVertex3f(point1.x, point1.y, point1.z);
  glVertex3f(point2.x, point2.y, point2.z);



  point1 = cor + dim1 + dim2;
  point2 = cor + dim1 + dim2 + dim3;
  glVertex3f(point1.x, point1.y, point1.z);
  glVertex3f(point2.x, point2.y, point2.z);
  point1 = cor + dim1 ;
  point2 = cor + dim2 + dim1;
  glVertex3f(point1.x, point1.y, point1.z);
  glVertex3f(point2.x, point2.y, point2.z);
  point1 = cor + dim1 + dim2;
  point2 = cor + dim2;
  glVertex3f(point1.x, point1.y, point1.z);
  glVertex3f(point2.x, point2.y, point2.z);

  glEnd();
}

Selection* HitBox::getSelection() {
	// variable for stocking openGL matrices
	GLdouble modelview[16];
	GLdouble projection[16];
	GLint viewport[4];

	glMatrixMode(GL_MODELVIEW);
	glPushMatrix();
	glLoadIdentity();
	glGetDoublev(GL_MODELVIEW_MATRIX, modelview); 
	glGetDoublev(GL_PROJECTION_MATRIX, projection);
	glGetIntegerv(GL_VIEWPORT, viewport); 
	glPopMatrix();
	Selection* selection = new Selection();
	
	GLdouble px,py,pz;
	glm::vec3 point;
	for (float i = 0.; i <= 1; i++)
		for (float j = 0.; j <= 1; j++)
			for (float k = 0.; k <= 1; k++) {
				point = m_corner + (i * m_dim1) + (j * m_dim2) + (k * m_dim3);
				gluProject(point.x,point.y,point.z,modelview,projection,viewport,&px,&py,&pz);
				// TODO : to be cleaned up using class Screen and not viewport array for screen dimension
				selection->addPoint(vec2(px / viewport[2] * 2.0  - 1.0 ,py / viewport[3] * 2.0 - 1.0));
	}
	point = m_corner;
	gluProject(point.x,point.y,point.z,modelview,projection,viewport,&px,&py,&pz);

	selection->addPoint(vec2(px / viewport[2] * 2.0 - 1.0 ,py / viewport[3] * 2.0 - 1.0));

	return selection;
}

inline vec4 vec4_from_vec3(vec3 v, float f) {
	return vec4(v.x, v.y, v.z, f);
}
inline vec3 vec3_from_vec4(vec4 v) {
	return vec3(v.x, v.y, v.z);
}


bool HitBox::collidePoint(vec3 point) {
	vec3 p0,p1,p2,p3;
	p0 = m_corner;
	p1 = m_dim1;
	p2 = m_dim2;
	p3 = m_dim3;
	vec3 v1, v2, v3;
	v1 = normalize(p1);
	v2 = normalize(p2); 
	v3 = normalize(p3);
	vec3 v = point - p0;
	float f1 = dot(v, v1);
	float f2 = dot(v, v2);
	float f3 = dot(v, v3);
	if ((f1 >= 0.0 && f1 <= dot(v1,p1)) && 
		(f2 >= 0.0 && f2 <= dot(v2,p2)) && 
		(f3 >= 0.0 && f3 <= dot(v3,p3))) return true;
	return false;

}

bool HitBox::collidePoint(vec3 point, glm::mat4 positionMatrix) {
	vec4 p0,p1,p2,p3;
	p0 = positionMatrix * vec4_from_vec3(m_corner,1.0);
	p1 = positionMatrix * vec4_from_vec3(m_corner + m_dim1,1.0);
	p2 = positionMatrix * vec4_from_vec3(m_corner + m_dim2,1.0);
	p3 = positionMatrix * vec4_from_vec3(m_corner + m_dim3,1.0);
	p0 /= p0.w; p1 /= p1.w; p2 /= p2.w; p3 /= p3.w;
	vec3 v1, v2, v3;
	v1 = vec3_from_vec4(normalize(p1 - p0));
	v2 = vec3_from_vec4(normalize(p2 - p0)); 
	v3 = vec3_from_vec4(normalize(p3 - p0));
	vec3 v = point - vec3_from_vec4(p0);
	float f1 = dot(v, v1);
	float f2 = dot(v, v2);
	float f3 = dot(v, v3);
	if ((f1 >= 0.0 && f1 <= dot(v1,v1)) && 
		(f2 >= 0.0 && f2 <= dot(v2,v2)) && 
		(f3 >= 0.0 && f3 <= dot(v3,v3))) return true;
	return false;
}

bool HitBox::collideHitBox(HitBox hb, glm::mat4 positionMatrix) {
	HitBox moved_hb = hb.getMovedHitBox(positionMatrix);
	HitBox this_moved = getMovedHitBox(positionMatrix);
	return moved_hb.collideHitBox(this_moved);
};

bool HitBox::collideHitBox(HitBox hb) {
	vec3 p1 = m_corner;
	if (hb.collidePoint(p1)) return true; 
	p1 = m_corner + m_dim1;
	if (hb.collidePoint(p1)) return true; 
	p1 = m_corner + m_dim1 + m_dim2;
	if (hb.collidePoint(p1)) return true; 
	p1 = m_corner + m_dim1 + m_dim3;
	if (hb.collidePoint(p1)) return true; 
	p1 = m_corner + m_dim1 + m_dim2 + m_dim3;
	if (hb.collidePoint(p1)) return true; 
	p1 = m_corner + m_dim2;
	if (hb.collidePoint(p1)) return true; 
	p1 = m_corner + m_dim2 + m_dim3;
	if (hb.collidePoint(p1)) return true; 
	p1 = m_corner + m_dim3;
	if (hb.collidePoint(p1)) return true; 
	return false;
}



Selection* HitBoxes::getSelection(glm::mat4 changeMatrix) {
	// hitboxes

	// we retrieve the current matrix
	GLdouble modelview[16];                                                          
	GLdouble projection[16];
	GLint viewport[4];

	glMatrixMode(GL_MODELVIEW);
	glPushMatrix();
	glLoadIdentity();
	glGetDoublev(GL_MODELVIEW_MATRIX, modelview); 
	glGetDoublev(GL_PROJECTION_MATRIX, projection);
	glGetIntegerv(GL_VIEWPORT, viewport); 
	glPopMatrix();

	Selection* selection = new Selection();
	list<HitBox>::iterator it;
	for (it = m_hitboxList.begin(); it != m_hitboxList.end(); it++) {
		vec3 point;
		GLdouble px,py,pz;

		for (float i = 0.0; i <= 1.; i+=1.0)
			for (float j = 0.0; j <= 1.; j+=1.0)
				for (float k = 0.0; k <= 1.; k+=1.0) {
					point = (*it).m_corner + i * (*it).m_dim1
											+ j * (*it).m_dim2
									  		+ k * (*it).m_dim3;
					vec4 pointv4 = changeMatrix * vec4(point.x, point.y, point.z, 1.0);
					pointv4 /= pointv4.w;
					gluProject(pointv4.x,pointv4.y,pointv4.z,modelview,projection,viewport,&px,&py,&pz);
					// TODO : to be   cleaned up using class Screen and not viewport array for screen dimension
					selection->addPoint(vec2(px / viewport[2] * 2.0  - 1.0 ,py / viewport[3] * 2.0 - 1.0));
		}
		// bug if corner is not added twice, TODO : to be corrected
		point = (*it).m_corner;
		gluProject(point.x,point.y,point.z,modelview,projection,viewport,&px,&py,&pz);
		selection->addPoint(vec2(px / viewport[2] * 2.0 - 1.0 ,py / viewport[3] * 2.0 - 1.0));

	}
	return selection;
}
